
#include "Matrix4.h"
#include "Vector3.h"
#include "Quaternion.h"

#include <algorithm>
#define _USE_MATH_DEFINES
#include <cmath>

namespace soft_renderer
{
	Matrix4::Matrix4()
	{
		data[0] = 1; data[1] = 0; data[2] = 0; data[3] = 0;
		data[4] = 0; data[5] = 1; data[6] = 0; data[7] = 0;
		data[8] = 0; data[9] = 0; data[10] = 1; data[11] = 0;
		data[12] = 0; data[13] = 0; data[14] = 0; data[15] = 1;
	}
	
	Matrix4::Matrix4(const Matrix4& m)
	{
		for (int i = 0; i < 16; i++)
		{
			data[i] = m.data[i];
		}
	}

	void Matrix4::set(double n11, double n12, double n13, double n14, double n21, double n22, double n23, double n24, double n31, double n32, double n33, double n34, double n41, double n42, double n43, double n44)
	{
		data[0] = n11; data[1] = n12; data[2] = n13; data[3] = n14;
		data[4] = n21; data[5] = n22; data[6] = n23; data[7] = n24;
		data[8] = n31; data[9] = n32; data[10] = n33; data[11] = n34;
		data[12] = n41; data[13] = n42; data[14] = n43; data[15] = n44;
	}

	void Matrix4::identity()
	{
		data[0] = 1; data[1] = 0; data[2] = 0; data[3] = 0;
		data[4] = 0; data[5] = 1; data[6] = 0; data[7] = 0;
		data[8] = 0; data[9] = 0; data[10] = 1; data[11] = 0;
		data[12] = 0; data[13] = 0; data[14] = 0; data[15] = 1;
	}

	void Matrix4::lookAt(Vector3 eye, Vector3 target, Vector3 up)
	{
		// http://eclipseflower.github.io/2017/01/11/View-Matrix/
		Vector3 x, y, z;
		z.subVectors(eye, target);
		if (z.lengthSq() == 0)
		{
			z.setZ(1);
		}
		z.normalize();

		x.crossVectors(up, z);
		if (x.lengthSq() == 0)
		{
			// n平行于up
			// 微调n，使其不与up完全平行
			if (std::abs(up.z() == 1))
			{
				z.setX(z.x() + 0.0001);
			}
			else
			{
				z.setZ(z.z() + 0.0001);
			}
			z.normalize();
			// 重新计算u
			x.crossVectors(up, z);
		}
		x.normalize();
		y.crossVectors(z, x);

		data[0] = x.x(); data[1] = x.y(); data[2] = x.z(); data[3] = 0;
		data[4] = y.x(); data[5] = y.y(); data[6] = y.z(); data[7] = 0;
		data[8] = z.x(); data[9] = z.y(); data[10] = z.z(); data[11] = 0;
		data[12] = 0; data[13] = 0; data[14] = 0; data[15] = 1;
	}

	void Matrix4::perspective(double fovy, double aspect, double near, double far)
	{
		// http://www.songho.ca/opengl/gl_projectionmatrix.html
		//double ymax, ymin, xmin, xmax;

		//ymax = near * tan(fovy * M_PI / 360);
		//ymin = -ymax;
		//xmin = ymin * aspect;
		//xmax = ymax * aspect;

		//frustum(xmin, xmax, ymin, ymax, near, far);
		double b = tan(fovy * M_PI / 180 / 2); // half height: h / 2
		double a = aspect * b; // half width: w / 2
		data[0] = near / a; data[1] = 0; data[2] = 0; data[3] = 0;
		data[4] = 0; data[5] = near / b; data[6] = 0; data[7] = 0;
		data[8] = 0; data[9] = 0; data[10] = -(far + near) / (far - near); data[11] = -2 * far * near / (far - near);
		data[12] = 0; data[13] = 0; data[14] = -1; data[15] = 0;
	}

	void Matrix4::orthographic(double left, double right, double bottom, double top, double near, double far)
	{
		double* te = data;
		double w = 1.0 / (right - left);
		double h = 1.0 / (top - bottom);
		double p = 1.0 / (far - near);

		double x = (right + left) * w;
		double y = (top + bottom) * h;
		double z = (far + near) * p;

		te[0] = 2 * w; te[1] = 0; te[2] = 0; te[3] = -x;
		te[4] = 0; te[5] = 2 * h; te[6] = 0; te[7] = -y;
		te[8] = 0; te[9] = 0; te[10] = -2 * p; te[11] = -z;
		te[12] = 0; te[13] = 0; te[15] = 0; te[15] = 1;
	}

	void Matrix4::setTranslation(Vector3 v)
	{
		data[3] = v.x();
		data[7] = v.y();
		data[11] = v.z();
	}

	void Matrix4::multiply(const Matrix4& m)
	{
		multiplyMatrices(*this, m);
	}

	void Matrix4::premultiply(const Matrix4& m)
	{
		multiplyMatrices(m, *this);
	}

	void Matrix4::multiplyMatrices(const Matrix4& a, const Matrix4& b)
	{
		const double* ae = a.data;
		const double* be = b.data;
		double* te = data;

		const double a11 = ae[0], a12 = ae[1], a13 = ae[2], a14 = ae[3];
		const double a21 = ae[4], a22 = ae[5], a23 = ae[6], a24 = ae[7];
		const double a31 = ae[8], a32 = ae[9], a33 = ae[10], a34 = ae[11];
		const double a41 = ae[12], a42 = ae[13], a43 = ae[14], a44 = ae[15];

		const double b11 = be[0], b12 = be[1], b13 = be[2], b14 = be[3];
		const double b21 = be[4], b22 = be[5], b23 = be[6], b24 = be[7];
		const double b31 = be[8], b32 = be[9], b33 = be[10], b34 = be[11];
		const double b41 = be[12], b42 = be[13], b43 = be[14], b44 = be[15];

		te[0] = a11 * b11 + a12 * b21 + a13 * b31 + a14 * b41; // c11
		te[1] = a11 * b12 + a12 * b22 + a13 * b32 + a14 * b42; // c12
		te[2] = a11 * b13 + a12 * b23 + a13 * b33 + a14 * b43;
		te[3] = a11 * b14 + a12 * b24 + a13 * b34 + a14 * b44;

		te[4] = a21 * b11 + a22 * b21 + a23 * b31 + a24 * b41;
		te[5] = a21 * b12 + a22 * b22 + a23 * b32 + a24 * b42;
		te[6] = a21 * b13 + a22 * b23 + a23 * b33 + a24 * b43;
		te[7] = a21 * b14 + a22 * b24 + a23 * b34 + a24 * b44;

		te[8] = a31 * b11 + a32 * b21 + a33 * b31 + a34 * b41;
		te[9] = a31 * b12 + a32 * b22 + a33 * b32 + a34 * b42;
		te[10] = a31 * b13 + a32 * b23 + a33 * b33 + a34 * b43;
		te[11] = a31 * b14 + a32 * b24 + a33 * b34 + a34 * b44;

		te[12] = a41 * b11 + a42 * b21 + a43 * b31 + a44 * b41;
		te[13] = a41 * b12 + a42 * b22 + a43 * b32 + a44 * b42;
		te[14] = a41 * b13 + a42 * b23 + a43 * b33 + a44 * b43;
		te[15] = a41 * b14 + a42 * b24 + a43 * b34 + a44 * b44;
	}

	void Matrix4::compose(Vector3 position, Quaternion quaternion, Vector3 scale)
	{
		double* te = data;

		const double x = quaternion.x(), y = quaternion.y(), z = quaternion.z(), w = quaternion.w();
		const double x2 = x + x, y2 = y + y, z2 = z + z;
		const double xx = x * x2, xy = x * y2, xz = x * z2;
		const double yy = y * y2, yz = y * z2, zz = z * z2;
		const double wx = w * x2, wy = w * y2, wz = w * z2;

		const double sx = scale.x(), sy = scale.y(), sz = scale.z();

		te[0] = (1 - (yy + zz)) * sx;
		te[4] = (xy + wz) * sx;
		te[8] = (xz - wy) * sx;
		te[12] = 0;

		te[1] = (xy - wz) * sy;
		te[5] = (1 - (xx + zz)) * sy;
		te[9] = (yz + wx) * sy;
		te[13] = 0;

		te[2] = (xz + wy) * sz;
		te[6] = (yz - wx) * sz;
		te[10] = (1 - (xx + yy)) * sz;
		te[14] = 0;

		te[3] = position.x();
		te[7] = position.y();
		te[11] = position.z();
		te[15] = 1;
	}

	void Matrix4::transpose()
	{
		double* te = data;
		double tmp;

		tmp = te[1]; te[1] = te[4]; te[4] = tmp;
		tmp = te[2]; te[2] = te[8]; te[8] = tmp;
		tmp = te[6]; te[6] = te[9]; te[9] = tmp;

		tmp = te[3]; te[3] = te[12]; te[12] = tmp;
		tmp = te[7]; te[7] = te[13]; te[13] = tmp;
		tmp = te[11]; te[11] = te[14]; te[14] = tmp;
	}

	// 行列式
	double Matrix4::determinant()
	{
		double* te = data;

		double n11 = te[0], n12 = te[1], n13 = te[2], n14 = te[3];
		double n21 = te[4], n22 = te[5], n23 = te[6], n24 = te[7];
		double n31 = te[8], n32 = te[9], n33 = te[10], n34 = te[11];
		double n41 = te[12], n42 = te[13], n43 = te[14], n44 = te[15];

		//TODO: make this more efficient
		//( based on http://www.euclideanspace.com/maths/algebra/matrix/functions/inverse/fourD/index.htm )

		return (
			n41 * (
				+n14 * n23 * n32
				- n13 * n24 * n32
				- n14 * n22 * n33
				+ n12 * n24 * n33
				+ n13 * n22 * n34
				- n12 * n23 * n34
				) +
			n42 * (
				+n11 * n23 * n34
				- n11 * n24 * n33
				+ n14 * n21 * n33
				- n13 * n21 * n34
				+ n13 * n24 * n31
				- n14 * n23 * n31
				) +
			n43 * (
				+n11 * n24 * n32
				- n11 * n22 * n34
				- n14 * n21 * n32
				+ n12 * n21 * n34
				+ n14 * n22 * n31
				- n12 * n24 * n31
				) +
			n44 * (
				-n13 * n22 * n31
				- n11 * n23 * n32
				+ n11 * n22 * n33
				+ n13 * n21 * n32
				- n12 * n21 * n33
				+ n12 * n23 * n31
				)

			);
	}

	void Matrix4::invert()
	{
		// based on http://www.euclideanspace.com/maths/algebra/matrix/functions/inverse/fourD/index.htm

		double* te = data;

		double n11 = te[0], n12 = te[1], n13 = te[2], n14 = te[3];
		double n21 = te[4], n22 = te[5], n23 = te[6], n24 = te[7];
		double n31 = te[8], n32 = te[9], n33 = te[10], n34 = te[11];
		double n41 = te[12], n42 = te[13], n43 = te[14], n44 = te[15];

		double t11 = n23 * n34 * n42 - n24 * n33 * n42 + n24 * n32 * n43 - n22 * n34 * n43 - n23 * n32 * n44 + n22 * n33 * n44;
		double t12 = n14 * n33 * n42 - n13 * n34 * n42 - n14 * n32 * n43 + n12 * n34 * n43 + n13 * n32 * n44 - n12 * n33 * n44;
		double t13 = n13 * n24 * n42 - n14 * n23 * n42 + n14 * n22 * n43 - n12 * n24 * n43 - n13 * n22 * n44 + n12 * n23 * n44;
		double t14 = n14 * n23 * n32 - n13 * n24 * n32 - n14 * n22 * n33 + n12 * n24 * n33 + n13 * n22 * n34 - n12 * n23 * n34;

		double det = n11 * t11 + n21 * t12 + n31 * t13 + n41 * t14;

		if (det == 0) return set(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);

		double detInv = 1 / det;

		te[0] = t11 * detInv;
		te[4] = (n24 * n33 * n41 - n23 * n34 * n41 - n24 * n31 * n43 + n21 * n34 * n43 + n23 * n31 * n44 - n21 * n33 * n44) * detInv;
		te[8] = (n22 * n34 * n41 - n24 * n32 * n41 + n24 * n31 * n42 - n21 * n34 * n42 - n22 * n31 * n44 + n21 * n32 * n44) * detInv;
		te[12] = (n23 * n32 * n41 - n22 * n33 * n41 - n23 * n31 * n42 + n21 * n33 * n42 + n22 * n31 * n43 - n21 * n32 * n43) * detInv;

		te[1] = t12 * detInv;
		te[5] = (n13 * n34 * n41 - n14 * n33 * n41 + n14 * n31 * n43 - n11 * n34 * n43 - n13 * n31 * n44 + n11 * n33 * n44) * detInv;
		te[9] = (n14 * n32 * n41 - n12 * n34 * n41 - n14 * n31 * n42 + n11 * n34 * n42 + n12 * n31 * n44 - n11 * n32 * n44) * detInv;
		te[13] = (n12 * n33 * n41 - n13 * n32 * n41 + n13 * n31 * n42 - n11 * n33 * n42 - n12 * n31 * n43 + n11 * n32 * n43) * detInv;

		te[2] = t13 * detInv;
		te[6] = (n14 * n23 * n41 - n13 * n24 * n41 - n14 * n21 * n43 + n11 * n24 * n43 + n13 * n21 * n44 - n11 * n23 * n44) * detInv;
		te[10] = (n12 * n24 * n41 - n14 * n22 * n41 + n14 * n21 * n42 - n11 * n24 * n42 - n12 * n21 * n44 + n11 * n22 * n44) * detInv;
		te[14] = (n13 * n22 * n41 - n12 * n23 * n41 - n13 * n21 * n42 + n11 * n23 * n42 + n12 * n21 * n43 - n11 * n22 * n43) * detInv;

		te[3] = t14 * detInv;
		te[7] = (n13 * n24 * n31 - n14 * n23 * n31 + n14 * n21 * n33 - n11 * n24 * n33 - n13 * n21 * n34 + n11 * n23 * n34) * detInv;
		te[11] = (n14 * n22 * n31 - n12 * n24 * n31 - n14 * n21 * n32 + n11 * n24 * n32 + n12 * n21 * n34 - n11 * n22 * n34) * detInv;
		te[15] = (n12 * n23 * n31 - n13 * n22 * n31 + n13 * n21 * n32 - n11 * n23 * n32 - n12 * n21 * n33 + n11 * n22 * n33) * detInv;
	}

}
